/******************************************************************************
 * Copyright 2022 The Rxbit Authors. All Rights Reserved.
 *
 * 理工睿行雷达
 *****************************************************************************/

#include "rxbit_radar.h"
#include "base/device_connect/radar/device_factory.h"

#include "yaml-cpp/yaml.h"

#include <chrono>
#include <fstream>
#include <thread>

namespace os
{
    namespace v2x
    {
        namespace device
        {
            /// @brief Epoll本地端口监听线程
            /// @return
            std::thread RxbitRadar::EpollProcessThread()
            {
                return std::thread([=]
                                   {
                        while (true)
                        {
                            int num = epoll_wait(epoll_fd, events, sizeof(events) / sizeof(events[0]), -1);
                            for (int i = 0; i < num; i++)
                            {
                                if (events[i].data.fd == server_fd)
                                {
                                    struct sockaddr_in client_addr;
                                    socklen_t len = sizeof(client_addr);
                                    int32_t client_fd = accept(server_fd, (struct sockaddr*)&client_addr, &len);

                                    if (client_fd == -1)
                                    {
                                        printf("Value of errno: %d\n", errno);
                                        printf("Socket error: %s\n", strerror(errno));
                                        printf("Accept client failed.\n");
                                    }

                                    struct timeval timeout = { 0, 1000 * 200 };
                                    setsockopt(client_fd, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(struct timeval));

                                    char ip[32] = { 0 };
                                    inet_ntop(AF_INET, &(client_addr.sin_addr.s_addr), ip, sizeof(ip));
                                    printf("New client connected! ip:%s port:%d\n", ip, ntohs(client_addr.sin_port));

                                    for (auto iter = radar_map.begin(); iter != radar_map.end(); iter++)
                                    {
                                        std::string current_ip = ip;
                                        if (current_ip == iter->second.ip)
                                        {
                                            printf("client in config file, add to epoll loop.\n");
                                            iter->second.fd = client_fd;
                                            iter->second.port= ntohs(client_addr.sin_port);

                                            fd_sn_map.insert(std::make_pair(client_fd, iter->first));

                                            struct epoll_event ev;
                                            ev.events = EPOLLIN;
                                            ev.data.fd = client_fd;
                                            epoll_ctl(epoll_fd, EPOLL_CTL_ADD, client_fd, &ev);
                                        }
                                    }
                                }
                                else if (events[i].events & EPOLLIN)
                                {
                                    std::shared_ptr<Msg> msg = std::make_shared<Msg>();
                                   
                                    int ret = Receive("rxbit:" , events[i].data.fd, (u_char*)msg->data, BUFFER_SZ, MSG_DONTWAIT);

                                    if(ret > 0)
                                    {
                                        msg->dataSize=ret;
                                        msg->fd = events[i].data.fd;
                                        data_queue.enqueue(msg);
                                    }
                                    else
                                    {
                                        printf("Client %d lost connection!\n", events[i].data.fd);
                                        close(events[i].data.fd);
                                        epoll_ctl(epoll_fd, EPOLL_CTL_DEL, events[i].data.fd, NULL);
                                    }
                                }
                            }
                        } });
            }

            std::thread RxbitRadar::SendMsgThread()
            {
                return std::thread([=]
                                   {

                        while (true)
                        {
                            std::shared_ptr<Msg> msg;
                            data_queue.wait_dequeue(msg);

                            printf("success dequeue a message!\n");

                            printf("tmsr_sn: %s\n", radar_map[fd_sn_map[msg->fd]].tmsr_sn.c_str());

                          
                            radar_map[fd_sn_map[msg->fd]].parser->FD= msg->fd;
                            radar_map[fd_sn_map[msg->fd]].parser->Invoke(msg->data,msg->dataSize);
                        } });
            }

            void RxbitRadar::DataParse(const u_char *data, int32_t dataSize, int32_t fd)
            {
                auto radar_data = std::make_shared<os::v2x::device::RadarObstacles>();

                auto radar_header = radar_data->mutable_header();
                radar_header->set_device_vendor(device_vendor);
                radar_header->set_device_firmware(device_firmware);
                radar_header->set_cross_id(cross_id);
                std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::system_clock::now().time_since_epoch());
                radar_header->set_device_timestamp(1607123541235);
                radar_header->set_message_timestamp(ms.count());
                radar_header->set_sequence_num(msg_sequence_num++);

                radar_header->set_device_id(radar_map[fd_sn_map[fd]].airos_sn);
                radar_header->set_device_ip(radar_map[fd_sn_map[fd]].ip);
                radar_header->set_device_port(std::to_string(radar_map[fd_sn_map[fd]].port));


                // objects
                RxbitRadarPackageHead *rxbit_header = (RxbitRadarPackageHead *)data;
                if (rxbit_header)
                {
                    if (rxbit_header->dataType == RXBIT_DATATYPE_TARGET)
                    {
                        RxbitRadarMsg65 *msg65 = (RxbitRadarMsg65 *)(data + sizeof(RxbitRadarPackageHead));
                        if (msg65 )
                        {
                            radar_header->set_longitude(msg65->longitude * 0.0000001);       
                            radar_header->set_latitude(msg65->latitude * 0.0000001);       
                            radar_header->set_altitude(msg65->altitude * 0.01); 
                            if (msg65->objNumber > 0)
                            {      
                                if(dataSize == (sizeof(RxbitRadarMsg65Item) * msg65->objNumber + 26+45))
                                {
                                    for (int i = 0; i < msg65->objNumber; i++)
                                    {
                                        // add a object to message
                                        auto obstacle= radar_data->add_obstacle();
                                   
                                        obstacle->set_id( msg65->lists[i].id);

                                        auto position = obstacle->mutable_position();
                                        position->set_x(msg65->lists[i].distX *0.1D);
                                        position->set_y(msg65->lists[i].distY *0.1D);
                                        position->set_z(0);

                                        obstacle->set_theta( msg65->lists[i].heading *0.01D);

                                        auto velocity = obstacle->mutable_velocity();
                                        velocity->set_x(msg65->lists[i].speedX *0.1D);
                                        velocity->set_y(msg65->lists[i].speedY *0.1D);
                                        velocity->set_z(0);

                                        // auto raw_velocity = obstacle->mutable_raw_velocity();
                                        // raw_velocity->set_x(msg65->lists[i].speedX *0.1D);
                                        // raw_velocity->set_y(msg65->lists[i].speedY *0.1D);
                                        // raw_velocity->set_z(0);

                                        obstacle->set_length( msg65->lists[i].length*0.1D);
                                        obstacle->set_width( msg65->lists[i].width*0.1D);
                                        obstacle->set_height( msg65->lists[i].height*0.1D);
                                        
                                        os::v2x::device::ObstacleType type= os::v2x::device::ObstacleType::UNKNOWN;
                                        switch (msg65->lists[i].type)
                                        {
                                            case 0x01:
                                            type=os::v2x::device::ObstacleType::CAR;
                                            break;
                                            case 0x02:
                                            type=os::v2x::device::ObstacleType::MEDIUMVEHICLE;
                                            break;
                                            case 0x03:
                                            case 0x06:
                                            type=os::v2x::device::ObstacleType::LARGEVEGICLE;
                                            break;
                                            case 0x04:
                                            type=os::v2x::device::ObstacleType::MOTORCYCLIST;
                                            break;
                                            case 0x05:
                                            type=os::v2x::device::ObstacleType::CYCLIST;
                                            break;
                                            default:
                                            type=os::v2x::device::ObstacleType::UNKNOWN_MOVABLE;
                                            break;
                                        }
                                        obstacle->set_type(type);

                                        auto acceleration = obstacle->mutable_acceleration();
                                        acceleration->set_x(msg65->lists[i].accX *0.01D);
                                        acceleration->set_y(msg65->lists[i].accY *0.01D);
                                        acceleration->set_z(0);

                                        obstacle->set_longitude( msg65->lists[i].longitude*0.0000001D);
                                        obstacle->set_latitude( msg65->lists[i].latitude*0.0000001D);
                                    }
                                }
                                else
                                {
                                    printf("radar object struct is unsafe, dataSize = %d != (sizeof(RxbitRadarMsg65Item) * msg65->nObjNum(%d) +26+45) \n", dataSize,msg65->objNumber);
                                }
                            }
                            // push
                            sender_(radar_data);
                        }
                    }
                }

            }

            bool RxbitRadar::Init(const std::string &config_file)
            {
                YAML::Node config;

                try
                {
                    config = YAML::LoadFile(config_file);
                }
                catch (...)
                {
                    printf("radar: Error load config file, please check config_file!\n");
                    return false;
                }

                if (config["radar"].size() <= 0)
                {
                    return false;
                }

                for (uint32_t i = 0; i < config["radar"].size(); i++)
                {
                    // RadarInformation ri(std::bind(&RxbitRadar::DataParse, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
                    RadarInformation ri;
                    ri.parser = std::make_shared<os::v2x::device::RxbitParser>([this](const u_char *data, int32_t dataSize, int32_t fd)
                                                                               { return DataParse(data, dataSize, fd); });
                    ri.ip = config["radar"][i]["ip"].as<std::string>();
                    ri.tmsr_sn = config["radar"][i]["tmsr_sn"].as<std::string>();
                    ri.airos_sn = config["radar"][i]["airos_sn"].as<std::string>();

                    radar_map.insert(std::pair<std::string, RadarInformation>(ri.tmsr_sn, ri));
                }

                server_port = config["local"]["port"].as<int32_t>();
                device_vendor = config["local"]["device_vendor"].as<std::string>();
                cross_id = config["local"]["cross_id"].as<std::string>();
                device_firmware = config["local"]["device_firmware"].as<std::string>();

                msg_sequence_num = 0;

                printf("rxbit radar initialized!\n");
                return true;
            }

            void RxbitRadar::Start()
            {
                server_fd = socket(AF_INET, SOCK_STREAM, 0);
                if (server_fd == -1)
                {
                    printf("Socket create failed.\n");
                    return;
                }

                struct sockaddr_in server_inet_addr;
                server_inet_addr.sin_family = AF_INET;
                server_inet_addr.sin_addr.s_addr = htonl(INADDR_ANY);
                server_inet_addr.sin_port = htons(server_port);

                int reuse_flag = 1;
                int flag_reuseaddr = setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &reuse_flag, sizeof(reuse_flag));
                int flag_reuseport = setsockopt(server_fd, SOL_SOCKET, SO_REUSEPORT, &reuse_flag, sizeof(reuse_flag));
                if (flag_reuseaddr < 0 || flag_reuseport < 0)
                {
                    printf("SO_REUSEADDR/SO_REUSEPORT set failed.\n");
                }

                if (bind(server_fd, (struct sockaddr *)&server_inet_addr, sizeof(server_inet_addr)) != 0)
                {
                    printf("Socket bind failed.\n");
                    return;
                }

                if (listen(server_fd, SOMAXCONN) != 0)
                {
                    printf("Socket listen failed.\n");
                    return;
                }

                epoll_fd = epoll_create(1024);
                if (epoll_fd == -1)
                {
                    printf("Epoll create failed.\n");
                    return;
                }
                else
                {
                    printf("Epoll created.\n");
                }

                struct epoll_event ev;
                ev.events = EPOLLIN;
                ev.data.fd = server_fd;
                epoll_ctl(epoll_fd, EPOLL_CTL_ADD, server_fd, &ev);

                EpollProcessThread().detach();
                SendMsgThread().detach();
            }

            V2XOS_RADAR_REG_FACTORY(RxbitRadar, "rxbit_radar");

        } // namespace device
    }     // namespace v2x
} // namespace os
